/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-03-26     huger       the first version
 */
#include <Magnetic/Magnetic.h>
#include <Bsp/bsp_i2c1.h>
#include <Bsp/bsp_spi2.h>


Magnetic::Magnetic()
{
    // TODO 自动生成的构造函数存根
    bsp_i2c1_init();

    rt_thread_mdelay(200);

    degree_start = magnetic_encoder_vaule_get() * per_degree;
    degree_now = degree_start;
    degree_preview = degree_now;
    rt_kprintf("degree_start : %.3f,degree_now : %.3f \n",degree_start,degree_now);
}

Magnetic::~Magnetic()
{
    // TODO 自动生成的析构函数存根
}

void Magnetic::CorrectDegree(void)
{
    correct_degree = magnetic_encoder_vaule_get()* per_degree - degree_start ;
    if(correct_degree <0)
    {
        correct_degree = correct_degree + 360.0;
    }
    else{
        //do nothing
    }
}

float Magnetic::magnetic_encoder_degree_get(void)
{
    CorrectDegree();
    /*
    //Quadrants
    4  |  1
    ---|---
    3  |  2
    */
    //Quadrant 1
    if(correct_degree >= 0 && correct_degree <=90)
    {
      quadrantNumber = 1;
    }

    //Quadrant 2
    if(correct_degree > 90 && correct_degree <=180)
    {
      quadrantNumber = 2;
    }

    //Quadrant 3
    if(correct_degree > 180 && correct_degree <=270)
    {
      quadrantNumber = 3;
    }

    //Quadrant 4
    if(correct_degree > 270 && correct_degree <360)
    {
      quadrantNumber = 4;
    }

    if(quadrantNumber != previousquadrantNumber) //if we changed quadrant
    {
      if(quadrantNumber == 1 && previousquadrantNumber == 4)
      {
        numberofTurns++; // 4 --> 1 transition: CW rotation
      }

      if(quadrantNumber == 4 && previousquadrantNumber == 1)
      {
        numberofTurns--; // 1 --> 4 transition: CCW rotation
      }
      //this could be done between every quadrants so one can count every 1/4th of transition

      previousquadrantNumber = quadrantNumber;  //update to the current quadrant

    }
    degree_now = (numberofTurns*360) + correct_degree + degree_start;
    //after we have the corrected angle and the turns, we can calculate the total absolute position
    return degree_now;//(numberofTurns*360) + correct_degree;; //number of turns (+/-) plus the actual angle within the 0-360 range
}

float Magnetic::magnetic_encoder_velocity_clc(unsigned short int time_sample)
{
    //time_sample : ms
    float velocity=0;  //rpm
//    degree_now = magnetic_encoder_degree_get();
    velocity = (degree_now - degree_preview ) * 1000000/time_sample;
    degree_preview = degree_now;
    return velocity;
}

unsigned int Magnetic::magnetic_encoder_vaule_get(void)
{
    return read_sensor_value(AS5600_ADDR,AS5600_GET_DATA);
}
